package states;

import gui.OpCom;
import regulators.BallAndBeamRegul;
import regulators.BeamRegul;
import se.lth.control.realtime.DigitalIn;
import se.lth.control.realtime.DigitalOut;
import stateMachine.StateMachine;
import monitor.StateVariableMonitor;

public class ResetState extends StateClass implements State {

	public ResetState(DigitalIn initialPosSensor, DigitalOut digitalSolenoid, BeamRegul beam,
			BallAndBeamRegul ballAndBeam, OpCom op, StateVariableMonitor stateVariableMonitor) {
		super(initialPosSensor, digitalSolenoid, beam, ballAndBeam, op, stateVariableMonitor);
	}

	double angle = beam.controllerStatMonitor.getBeamAngle1();
	double refDeltaBeam = stateVariableMonitor.getRefdeltabeam();
	int counter;

	@Override
	public void initialExcecution() {
		op.displayStep("ResetState");
		counter = 0;
		ballAndBeam.resetBallSize();
		try{
			digitalSolenoid.set(true);
		} catch (Exception e) {
			e.printStackTrace();
		}

		beam.setRef(angle);
		if(!beam.isRunning()) {
			beam.restart();
		}

		if(angle > 1.5-0.4){
			refDeltaBeam = refDeltaBeam*-1;
		}

	}

	@Override
	public void excecute() {	
		angle += refDeltaBeam*5;
		beam.setRef(angle);
	}

	@Override
	public boolean stateOfExcecution() {

		double currentAngle = beam.controllerStatMonitor.getBeamAngle1();
		if(currentAngle >= 1.5 -0.4 && currentAngle <= 1.5 + 0.4  ) {
			return true;
		}
		return false;
	}


	@Override
	public void nextState() {
		beam.shutDown();
		setCurrentState( new InitialState(initialPosSensor, digitalSolenoid, beam, ballAndBeam, op, stateVariableMonitor));
	}

	@Override
	public void suspend() {
		beam.shutDown();	
	}

	@Override
	public void resume() {
		beam.restart();	
	}


}